import math
import os
from ament_index_python import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node, PushROSNamespace
import numpy as np


def generate_launch_description():
    """
    创建一个带有两个使用外部控制逻辑的机器人仿真环境
    为每个机器人配置一个外部控制器和轨迹跟踪器
    创建局部路径规划器，生成局部轨迹
    """


    package_path = get_package_share_directory("robot_define")
    urdf_file = os.path.join(package_path, "urdf", "robot_simple.urdf")
    with open(urdf_file, "r") as infp:
        urdf_content = infp.read()

    chassis = Node(
        package="chassis",
        executable="chassis_server",
        name="chassis_server",
        output="screen",
        parameters=[
            {"force_scale": 500.0},
            {"speed_limit": 3.0},
        ],
    )

    controller = Node(
        package="controller",
        executable="tri_pid_lc",
        name="state_controller",
        output="screen",
        parameters=[
            {"gyro_pid": [0.6, 0.1, 0.0, 0.5]}, 
            {"angle_pid": [3.0, 0.1, 0.0, 5.0]},
            {"speed_pid": [30.0, 10.0, 0.0, 0.1]},  
            {"rotate_pid": [0.5, 0.2, 0.0, 20.0]},  
            {"gyro_angle_forward": 0.5}, 
            {"max_angle": 5.0},
            {"max_gyro": 10.0},
            {"max_speed": 1.0},
            {"gyro_filter": 0.8},
            {"angle_filter": 0.8},
            {"speed_filter": 0.95},
            {"gyroz_filter": 0.2},
            {"angle_offset": 1.0},
            {"state_rate": -1.0},
        ],
    )

    odom_node = Node(
        package="controller",
        executable="odom",
        name="real_odom",
        output="screen",
    )

    follow_mpc_node = Node(
        package="controller_py",
        executable="mpc_path_tracker_lc",
        name="follow_controller",
        output="screen",
        parameters=[
            {"max_gyro": 30 / 180.0 * math.pi},
            {"mat_q": [10.0, 10.0, 10.0, 10.0]},
            {"mat_r": [1.0, 1.0]},
        ],
    )

    local_path_planer = Node(
        package="path_planer",
        executable="local_path_planer_lc",
        name="local_planer",
        output="screen",
        parameters=[
            {"radius": 0.1},
            {"invert": False},
            {"start_vec": 0.5},
            {"dock_vec": 0.1},
        ],
        remappings=[
            ("odom_start", "odom"),
            ("odom_end", "target_odom"),
            ("local_path", "reference_path"),
            ("local_vec", "reference_velocity"),
        ],
    )

    core = Node(
        package="core",
        executable="core",
        name="core",
        output="screen",
        parameters=[
            {
                "prefix": "/robot2",
            }
        ],
    )

    state_publisher_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher_node",
        output="screen",
        parameters=[
            {"robot_description": urdf_content},
            {"frame_prefix": "/robot2/"},
        ],
    )

    return LaunchDescription(
        [
            PushROSNamespace("robot2"),
            chassis,
            controller,
            # odom_node,
            # connector,
            follow_mpc_node,
            local_path_planer,
            core,
        ]
    )
